import lejos.nxt.*;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.TachoPilot;

/**
 * Code voor calibratie en demo 1
 * 
 * @author Filip, Roel
 * 
 * 
 */
public class square
{
	static float inchToCm =2.536f; //constante om default inch om te zetten naar cm, inclusief een factor ter calibratie
    Pilot pilot ;
    
    
    /**
     * Methode om regelmatige veelhoek te maken met aantal hoeken en lengte zijde als parameter
     * 
     * @param length Gewenste lengte zijde veelhoek
     * @param veelhoek Aantal gewenste hoeken
     */
    public void  drawSquare(float length, int veelhoek) 
    {
    	
    	float graden = ((float)veelhoek-2f)*180f;
    	pilot.setTurnSpeed(70); //trager dan default tegen slippen, ter nauwkeurigheid
    	pilot.setMoveSpeed(7f);
        for(int i = 0; i<veelhoek; i++)
        {
            pilot.travel(length);
            float hoek = 180-graden/veelhoek;
            pilot.rotate(correctHoek(hoek)); 
            
        }
    }
    public void  turn(int secties)  //methode om te controleren of een gekozen hoek bij herhaling tot 360° volledig rond gaat
    {
        float hoek = 360/secties;
        
    	for(int i = 0; i<secties; i++){
    		pilot.rotate(correctHoek(hoek));
    	}

    	
        
    }
    /**
     * empirisch gedefinieerde methode om een correctie uit te voeren om de methode turn te laten kloppen, voor hoeken van 180 tem 15 graden
     * 
     * @param hoek hoek die gewenst is als resultaat
     * @return Gecorrigeerde hoek om gewenste hoek werkelijk te krijgen
     */
    private float correctHoek(float hoek){ 
    	float correctie = 0;
        if(Math.abs(hoek)< 180){
        	//correctie =Math.signum(hoek)/(0.689f*hoek+4.12f)*15f;
        	correctie = -0.0096f*hoek-0.052f;	//interpolatie door onderstaande punten
        }
        if(Math.abs(hoek)== 90){  //individuele correctie voor 90 graden
        	correctie =Math.signum(hoek)*(-1f);
        }
        if(Math.abs(hoek)== 72){  //individuele correctie voor 72 graden - vijfhoek
        	correctie =Math.signum(hoek)*(-0.65f);
        }
        if(Math.abs(hoek)== 60){  //individuele correctie voor 60 graden - zeshoek
        	correctie =Math.signum(hoek)*(-0.6f);
        }
        if(Math.abs(hoek)== 45){  //individuele correctie voor 45 graden - acht hoek
        	correctie =Math.signum(hoek)*(-0.5f);
        }
        if(Math.abs(hoek)== 36){  //individuele correctie voor 36 graden - tien hoek
        	correctie =Math.signum(hoek)*(-0.38f);
        }

        if(Math.abs(hoek)== 22.5f){  //individuele correctie voor 22.5 graden - zestienhoek
        	correctie =Math.signum(hoek)*(-0.31f);
        }
        if(Math.abs(hoek)== 15f){  //individuele correctie voor 15 graden - 24hoek
        	correctie =Math.signum(hoek)*(0f);
        }
        return hoek + correctie;
    }
    
    
    /**
     * Eenvoudige methode om twee meter af te leggen
     */
    public void twoMeters()
    {
    	float hoek=90;
    	pilot.travel(-100f/inchToCm);
    	pilot.rotate(correctHoek(hoek));
    	pilot.travel(-10f/inchToCm);
    	pilot.rotate(correctHoek(hoek));
    	pilot.travel(-100f/inchToCm);
    	pilot.rotate(correctHoek(hoek));
    	pilot.travel(-10f/inchToCm);
    	pilot.rotate(correctHoek(hoek));

    }
    
    /**
     * Main, declaratie pilot en uitvoer commandos
     * 
     * @param args Nog niet in gebruik
     */
    public static void main(String[] args)
    {
        square sq = new square();
        sq.pilot = new TachoPilot(2.184f, 4.233f, Motor.A, Motor.C, true);

        sq.drawSquare(-200f/inchToCm, 2);
        
        //sq.turn(4);
        //sq.twoMeters();
    }
}


